
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_position_control.c
  * @author     baiyang
  * @date       2021-8-29
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mc_position_control.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
/*-----------------------------------macro------------------------------------*/
// default gains for Copter / TradHeli
#define POSCONTROL_POS_Z_P                    1.0f    // vertical position controller P gain default
#define POSCONTROL_VEL_Z_P                    5.0f    // vertical velocity controller P gain default
#define POSCONTROL_VEL_Z_IMAX                 1000.0f // vertical velocity controller IMAX gain default
#define POSCONTROL_VEL_Z_FILT_HZ              5.0f    // vertical velocity controller input filter
#define POSCONTROL_VEL_Z_FILT_D_HZ            5.0f    // vertical velocity controller input filter for D
#define POSCONTROL_ACC_Z_P                    0.5f    // vertical acceleration controller P gain default
#define POSCONTROL_ACC_Z_I                    1.0f    // vertical acceleration controller I gain default
#define POSCONTROL_ACC_Z_D                    0.0f    // vertical acceleration controller D gain default
#define POSCONTROL_ACC_Z_IMAX                 800     // vertical acceleration controller IMAX gain default
#define POSCONTROL_ACC_Z_FILT_HZ              20.0f   // vertical acceleration controller input filter default
#define POSCONTROL_ACC_Z_DT                   0.0025f // vertical acceleration controller dt default
#define POSCONTROL_POS_XY_P                   1.0f    // horizontal position controller P gain default
#define POSCONTROL_VEL_XY_P                   2.0f    // horizontal velocity controller P gain default
#define POSCONTROL_VEL_XY_I                   1.0f    // horizontal velocity controller I gain default
#define POSCONTROL_VEL_XY_D                   0.5f    // horizontal velocity controller D gain default
#define POSCONTROL_VEL_XY_IMAX                1000.0f // horizontal velocity controller IMAX gain default
#define POSCONTROL_VEL_XY_FILT_HZ             5.0f    // horizontal velocity controller input filter
#define POSCONTROL_VEL_XY_FILT_D_HZ           5.0f    // horizontal velocity controller input filter for D

// vibration compensation gains
#define POSCONTROL_VIBE_COMP_P_GAIN 0.250f
#define POSCONTROL_VIBE_COMP_I_GAIN 0.125f
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void posctrl_init_xy(Position_ctrl* pos_ctrl);
void posctrl_init_ekf_xy_reset(Position_ctrl* pos_ctrl);
bool posctrl_has_good_timing();
void posctrl_init_z(Position_ctrl* pos_ctrl);
float posctrl_get_throttle_with_vibration_override(Position_ctrl* pos_ctrl);
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
void posctrl_ctor(Position_ctrl* pos_ctrl, ahrs_view* ahrs,Motors_HandleTypeDef* motors, Attitude_ctrl* attitude_control, float dt)
{
    pos_ctrl->_ahrs   = ahrs;
    pos_ctrl->_motors = motors;
    pos_ctrl->_attitude_control = attitude_control;

    p_1d_ctrl_ctor(&pos_ctrl->_p_pos_z, POSCONTROL_POS_Z_P, dt);

    pid_basic_ctrl_ctor(&pos_ctrl->_pid_vel_z, POSCONTROL_VEL_Z_P,
                        0.0f,
                        0.0f,
                        0.0f,
                        POSCONTROL_VEL_Z_IMAX,
                        POSCONTROL_VEL_Z_FILT_HZ,
                        POSCONTROL_VEL_Z_FILT_D_HZ,
                        dt);

    pid_ctrl_ctor(&pos_ctrl->_pid_accel_z, POSCONTROL_ACC_Z_P,
                        POSCONTROL_ACC_Z_I,
                        POSCONTROL_ACC_Z_D,
                        0.0f,
                        POSCONTROL_ACC_Z_IMAX,
                        0.0f,
                        POSCONTROL_ACC_Z_FILT_HZ,
                        0.0f,
                        dt);

    p_2d_ctrl_ctor(&pos_ctrl->_p_pos_xy, POSCONTROL_POS_XY_P, dt);

    pid_2d_ctrl_ctor(&pos_ctrl->_pid_vel_xy, POSCONTROL_VEL_XY_P,
                        POSCONTROL_VEL_XY_I,
                        POSCONTROL_VEL_XY_D,
                        0.0f,
                        POSCONTROL_VEL_XY_IMAX,
                        POSCONTROL_VEL_XY_FILT_HZ,
                        POSCONTROL_VEL_XY_FILT_D_HZ,
                        dt);

    pos_ctrl->_dt = dt;

    pos_ctrl->_vel_max_down_cms  = POSCONTROL_SPEED_DOWN;
    pos_ctrl->_vel_max_up_cms    = POSCONTROL_SPEED_UP;
    pos_ctrl->_vel_max_xy_cms    = POSCONTROL_SPEED;
    pos_ctrl->_accel_max_z_cmss  = POSCONTROL_ACCEL_Z;
    pos_ctrl->_accel_max_xy_cmss = POSCONTROL_ACCEL_XY;
    pos_ctrl->_jerk_max_xy_cmsss = POSCONTROL_JERK_XY * 100.0f;
    pos_ctrl->_jerk_max_z_cmsss  = POSCONTROL_JERK_Z * 100.0f;

    pos_ctrl->_lean_angle_max    = 0.0f;
    pos_ctrl->_shaping_jerk_xy   = POSCONTROL_JERK_XY;
    pos_ctrl->_shaping_jerk_z    = POSCONTROL_JERK_Z;

    // initialise flags
    pos_ctrl->_limit.pos_xy = true;
    pos_ctrl->_limit.pos_up = true;
    pos_ctrl->_limit.pos_down = true;

    pos_ctrl->_vel_z_control_ratio = 2.0f;

    pos_ctrl->_psc_data.ahrsGndSpdLimit = 400.0f;
    pos_ctrl->_psc_data.ahrsControlScaleXY = 1.0f;
    pos_ctrl->_psc_data.controlScaleZ = 1.0f;

    posctrl_assign_param(pos_ctrl);
}

/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum jerk parameter and the velocity and acceleration limits set using the function set_max_speed_accel_xy.
///     The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
///     The jerk limit also defines the time taken to achieve the maximum acceleration.
///     The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
void posctrl_input_pos_xyz(Position_ctrl* pos_ctrl, const Vector3p* pos, float pos_offset_z, float pos_offset_z_buffer)
{
    // Terrain following velocity scalar must be calculated before we remove the position offset
    const float offset_z_scaler = posctrl_pos_offset_z_scaler(pos_ctrl, pos_offset_z, pos_offset_z_buffer);

    // remove terrain offsets for flat earth assumption
    pos_ctrl->_pos_target.z -= pos_ctrl->_pos_offset_z;
    pos_ctrl->_vel_desired.z -= pos_ctrl->_vel_offset_z;
    pos_ctrl->_accel_desired.z -= pos_ctrl->_accel_offset_z;

    // calculated increased maximum acceleration if over speed
    float accel_z_cmss = pos_ctrl->_accel_max_z_cmss;
    if (pos_ctrl->_vel_desired.z < pos_ctrl->_vel_max_down_cms && !math_flt_zero(pos_ctrl->_vel_max_down_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_down_cms;
    }
    if (pos_ctrl->_vel_desired.z > pos_ctrl->_vel_max_up_cms && !math_flt_zero(pos_ctrl->_vel_max_up_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_up_cms;
    }

    control_update_pos_vel_accel_xy((Vector2f_t *)(&pos_ctrl->_pos_target), 
                                    (Vector2f_t *)(&pos_ctrl->_vel_desired), 
                                    (Vector2f_t *)(&pos_ctrl->_accel_desired), 
                                    pos_ctrl->_dt, 
                                    *((Vector2f_t *)(&pos_ctrl->_limit_vector)));

    // adjust desired altitude if motors have not hit their limits
    control_update_pos_vel_accel(&pos_ctrl->_pos_target.z, 
                                 &pos_ctrl->_vel_desired.z, 
                                 pos_ctrl->_accel_desired.z, 
                                 pos_ctrl->_dt, 
                                 pos_ctrl->_limit_vector.z);

    // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos
    float vel_max_xy_cms = 0.0f;
    float vel_max_z_cms = 0.0f;
    Vector3f_t dest_vector;
    vec3_sub(&dest_vector, pos, &pos_ctrl->_pos_target);
    if (math_flt_positive(vec3_length_squared(&dest_vector))) {
        vec3_norm(&dest_vector, &dest_vector);
        float dest_vector_xy_length = vec3_length_xy(&dest_vector);

        float vel_max_cms = control_kinematic_limit(dest_vector, pos_ctrl->_vel_max_xy_cms, pos_ctrl->_vel_max_up_cms, pos_ctrl->_vel_max_down_cms);
        vel_max_xy_cms = vel_max_cms * dest_vector_xy_length;
        vel_max_z_cms = fabsf(vel_max_cms * dest_vector.z);
    }

    // reduce speed if we are reaching the edge of our vertical buffer
    vel_max_xy_cms *= offset_z_scaler;

    Vector2f_t vel = VECTOR2F_DEFAULT_VALUE;
    Vector2f_t accel = VECTOR2F_DEFAULT_VALUE;
    control_shape_pos_vel_accel_xy((Vector2p*)pos, &vel, &accel, 
                                   (Vector2p *)(&pos_ctrl->_pos_target), 
                                   (Vector2f_t *)(&pos_ctrl->_vel_desired), 
                                   (Vector2f_t *)(&pos_ctrl->_accel_desired),
                                   vel_max_xy_cms, 
                                   pos_ctrl->_accel_max_xy_cmss, 
                                   pos_ctrl->_jerk_max_xy_cmsss, 
                                   pos_ctrl->_dt, 
                                   false);

    float posz = pos->z;
    control_shape_pos_vel_accel(posz, 0.0f, 0.0f,
                        pos_ctrl->_pos_target.z, pos_ctrl->_vel_desired.z, &pos_ctrl->_accel_desired.z,
                        -vel_max_z_cms, vel_max_z_cms,
                        -math_constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
                        pos_ctrl->_jerk_max_z_cmsss, pos_ctrl->_dt, false);

    // update the vertical position, velocity and acceleration offsets
    posctrl_update_pos_offset_z(pos_ctrl, pos_offset_z);

    // add terrain offsets
    pos_ctrl->_pos_target.z += pos_ctrl->_pos_offset_z;
    pos_ctrl->_vel_desired.z += pos_ctrl->_vel_offset_z;
    pos_ctrl->_accel_desired.z += pos_ctrl->_accel_offset_z;
}

///
/// Lateral position controller
///

/// set_max_speed_accel_xy - set the maximum horizontal speed in cm/s and acceleration in cm/s/s
///     This function only needs to be called if using the kinematic shaping.
///     This can be done at any time as changes in these parameters are handled smoothly
///     by the kinematic shaping.
void posctrl_set_max_speed_accel_xy(Position_ctrl* pos_ctrl, float speed_cms, float accel_cmss)
{
    pos_ctrl->_vel_max_xy_cms = speed_cms;
    pos_ctrl->_accel_max_xy_cmss = accel_cmss;

    // ensure the horizontal jerk is less than the vehicle is capable of
    const float jerk_max_cmsss = MIN(attctrl_get_ang_vel_roll_max_rads(pos_ctrl->_attitude_control), attctrl_get_ang_vel_pitch_max_rads(pos_ctrl->_attitude_control)) * GRAVITY_MSS * 100.0f;
    const float snap_max_cmssss = MIN(attctrl_get_accel_roll_max_radss(pos_ctrl->_attitude_control), attctrl_get_accel_pitch_max_radss(pos_ctrl->_attitude_control)) * GRAVITY_MSS * 100.0f;

    // get specified jerk limit
    pos_ctrl->_jerk_max_xy_cmsss = pos_ctrl->_shaping_jerk_xy * 100.0f;

    // limit maximum jerk based on maximum angular rate
    if (math_flt_positive(jerk_max_cmsss) && attctrl_get_bf_feedforward(pos_ctrl->_attitude_control)) {
        pos_ctrl->_jerk_max_xy_cmsss = MIN(pos_ctrl->_jerk_max_xy_cmsss, jerk_max_cmsss);
    }

    // limit maximum jerk to maximum possible average jerk based on angular acceleration
    if (math_flt_positive(snap_max_cmssss) && attctrl_get_bf_feedforward(pos_ctrl->_attitude_control)) {
        pos_ctrl->_jerk_max_xy_cmsss = MIN(0.5 * math_sqrtf(pos_ctrl->_accel_max_xy_cmss * snap_max_cmssss), pos_ctrl->_jerk_max_xy_cmsss);
    }
}

/// set_max_speed_accel_xy - set the position controller correction velocity and acceleration limit
///     This should be done only during initialisation to avoid discontinuities
void posctrl_set_correction_speed_accel_xy(Position_ctrl* pos_ctrl, float speed_cms, float accel_cmss)
{
    p_2d_ctrl_set_limits(&pos_ctrl->_p_pos_xy, speed_cms, accel_cmss, 0.0f);
}

/// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
void posctrl_init_xy_controller(Position_ctrl* pos_ctrl)
{
    posctrl_init_xy(pos_ctrl);

    // set resultant acceleration to current attitude target
    Vector3f_t accel_target;
    posctrl_lean_angles_to_accel_xy(pos_ctrl, &accel_target.x, &accel_target.y);

    
    const Vector2f_t reset_i = {accel_target.x - pos_ctrl->_accel_desired.x,
                              accel_target.y - pos_ctrl->_accel_desired.y};
    pid_2d_set_integrator(&pos_ctrl->_pid_vel_xy, &reset_i);
}

/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
///     This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
///     The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void posctrl_init_xy_controller_stopping_point(Position_ctrl* pos_ctrl)
{
    posctrl_init_xy(pos_ctrl);

    posctrl_get_stopping_point_xy_cm(pos_ctrl, (Vector2p *)&pos_ctrl->_pos_target);
    vec2_zero((Vector2f_t*)&pos_ctrl->_vel_desired);
    vec2_zero((Vector2f_t*)&pos_ctrl->_accel_desired);

    const Vector2f_t reset_i = {pos_ctrl->_accel_target.x, pos_ctrl->_accel_target.y};
    pid_2d_set_integrator(&pos_ctrl->_pid_vel_xy, &reset_i);
}

// relax_velocity_controller_xy - initialise the position controller to the current position and velocity with decaying acceleration.
///     This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void posctrl_relax_velocity_controller_xy(Position_ctrl* pos_ctrl)
{
    posctrl_init_xy(pos_ctrl);

    // decay resultant acceleration and therefore current attitude target to zero
    float decay = 1.0 - pos_ctrl->_dt / (pos_ctrl->_dt + POSCONTROL_RELAX_TC);

    vec2_mult((Vector2f_t*)&pos_ctrl->_accel_target,
              (Vector2f_t*)&pos_ctrl->_accel_target,
              decay);
    
    const Vector2f_t reset_i = {pos_ctrl->_accel_target.x - pos_ctrl->_accel_desired.x,
                                pos_ctrl->_accel_target.y - pos_ctrl->_accel_desired.y};
    
    pid_2d_set_integrator(&pos_ctrl->_pid_vel_xy, &reset_i);
}

/// init_xy - initialise the position controller to the current position, velocity and acceleration.
///     This function is private and contains all the shared xy axis initialisation functions
void posctrl_init_xy(Position_ctrl* pos_ctrl)
{
    // set roll, pitch lean angle targets to current attitude
    const Vector3f_t att_target_euler_cd = attctrl_get_att_target_euler_cd(pos_ctrl->_attitude_control);
    pos_ctrl->_roll_target = att_target_euler_cd.x;
    pos_ctrl->_pitch_target = att_target_euler_cd.y;
    pos_ctrl->_yaw_target = att_target_euler_cd.z; // todo: this should be thrust vector heading, not yaw.
    pos_ctrl->_yaw_rate_target = 0.0f;

    const Vector3f_t* curr_pos = &pos_ctrl->_ahrs->relpos_cm;
    pos_ctrl->_pos_target.x = curr_pos->x;
    pos_ctrl->_pos_target.y = curr_pos->y;

    const Vector3f_t* curr_vel = &pos_ctrl->_ahrs->velocity_cm;
    pos_ctrl->_vel_desired.x = curr_vel->x;
    pos_ctrl->_vel_desired.y = curr_vel->y;
    pos_ctrl->_vel_target.x = curr_vel->x;
    pos_ctrl->_vel_target.y = curr_vel->y;

    const Vector3f_t curr_accel = {pos_ctrl->_ahrs->accel_ef.x * 100.0f,
                                   pos_ctrl->_ahrs->accel_ef.y * 100.0f,
                                   pos_ctrl->_ahrs->accel_ef.z * 100.0f};

    pos_ctrl->_accel_desired.x = curr_accel.x;
    pos_ctrl->_accel_desired.y = curr_accel.y;
    
    vec3_limit_length_xy(&pos_ctrl->_accel_desired, pos_ctrl->_accel_max_xy_cmss);

    posctrl_lean_angles_to_accel_xy(pos_ctrl, &pos_ctrl->_accel_target.x, &pos_ctrl->_accel_target.y);

    // initialise I terms from lean angles
    const Vector2f_t reset_i = {pos_ctrl->_accel_target.x - pos_ctrl->_accel_desired.x,
                          pos_ctrl->_accel_target.y - pos_ctrl->_accel_desired.y};

    pid_2d_ctrl_reset_filter(&pos_ctrl->_pid_vel_xy);
    pid_2d_set_integrator(&pos_ctrl->_pid_vel_xy, &reset_i);

    // initialise ekf xy reset handler
    posctrl_init_ekf_xy_reset(pos_ctrl);

    // initialise z_controller time out
    pos_ctrl->_last_update_xy_us = time_micros64();
}

/// input_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
///     The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
///     The jerk limit also defines the time taken to achieve the maximum acceleration.
void posctrl_input_accel_xy(Position_ctrl* pos_ctrl, const Vector3f_t* accel)
{
    // TODO: check for ekf xy position reset
    //handle_ekf_xy_reset();

    control_update_pos_vel_accel_xy((Vector2p*)&pos_ctrl->_pos_target,
                                    (Vector2f_t*)&pos_ctrl->_vel_desired,
                                    (Vector2f_t*)&pos_ctrl->_accel_desired,
                                    pos_ctrl->_dt,
                                    (*(Vector2f_t*)&pos_ctrl->_limit_vector));
    
    control_shape_accel_xy2(accel, &pos_ctrl->_accel_desired, pos_ctrl->_jerk_max_xy_cmsss, pos_ctrl->_dt);
}

/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
///     The vel is projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_vel_accel_xy(Position_ctrl* pos_ctrl, Vector2f_t* vel, const Vector2f_t* accel, bool limit_output)
{
    control_update_pos_vel_accel_xy((Vector2p*)&pos_ctrl->_pos_target,
                                    (Vector2f_t*)&pos_ctrl->_vel_desired,
                                    (Vector2f_t*)&pos_ctrl->_accel_desired,
                                    pos_ctrl->_dt,
                                    (*(Vector2f_t*)&pos_ctrl->_limit_vector));

    control_shape_vel_accel_xy(vel, accel, (Vector2f_t*)&pos_ctrl->_vel_desired, (Vector2f_t*)&pos_ctrl->_accel_desired,
        pos_ctrl->_accel_max_xy_cmss, pos_ctrl->_jerk_max_xy_cmsss, pos_ctrl->_dt, limit_output);

    Vector2f_t limit_vector = VECTOR2F_DEFAULT_VALUE;
    control_update_vel_accel_xy(vel, accel, pos_ctrl->_dt, limit_vector);
}

/// input_pos_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
///     The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The function alters the pos and vel to be the kinematic path based on accel
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_pos_vel_accel_xy(Position_ctrl* pos_ctrl, Vector2p* pos, Vector2f_t* vel, const Vector2f_t* accel, bool limit_output)
{
    control_update_pos_vel_accel_xy((Vector2p*)&pos_ctrl->_pos_target,
                                    (Vector2f_t*)&pos_ctrl->_vel_desired,
                                    (Vector2f_t*)&pos_ctrl->_accel_desired,
                                    pos_ctrl->_dt,
                                    (*(Vector2f_t*)&pos_ctrl->_limit_vector));

    control_shape_pos_vel_accel_xy(pos, vel, accel, (Vector2p*)&pos_ctrl->_pos_target, 
                           (Vector2f_t*)&pos_ctrl->_vel_desired, (Vector2f_t*)&pos_ctrl->_accel_desired,
                           pos_ctrl->_vel_max_xy_cms, pos_ctrl->_accel_max_xy_cmss, pos_ctrl->_jerk_max_xy_cmsss, 
                           pos_ctrl->_dt, limit_output);

    Vector2f_t limit_vector = VECTOR2F_DEFAULT_VALUE;
    control_update_pos_vel_accel_xy(pos, vel, accel, pos_ctrl->_dt, limit_vector);
}

// is_active_xy - returns true if the xy position controller has been run in the previous 5 loop times
bool posctrl_is_active_xy(Position_ctrl* pos_ctrl)
{
    return ((time_micros64() - pos_ctrl->_last_update_xy_us) <= pos_ctrl->_dt * 5000000.0);
}

/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
void posctrl_stop_pos_xy_stabilisation(Position_ctrl* pos_ctrl)
{
    const Vector3f_t curr_pos = pos_ctrl->_ahrs->relpos_cm;

    pos_ctrl->_pos_target.x = curr_pos.x;
    pos_ctrl->_pos_target.y = curr_pos.y;
}

/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system
void posctrl_stop_vel_xy_stabilisation(Position_ctrl* pos_ctrl)
{
    pos_ctrl->_pos_target.x = pos_ctrl->_ahrs->relpos_cm.x;
    pos_ctrl->_pos_target.y = pos_ctrl->_ahrs->relpos_cm.y;

    pos_ctrl->_vel_desired.x = pos_ctrl->_ahrs->velocity_cm.x;
    pos_ctrl->_vel_desired.y = pos_ctrl->_ahrs->velocity_cm.y;
    
    // with zero position error _vel_target = _vel_desired
    pos_ctrl->_vel_target.x = pos_ctrl->_ahrs->velocity_cm.x;
    pos_ctrl->_vel_target.y = pos_ctrl->_ahrs->velocity_cm.y;

    // initialise I terms from lean angles
    pid_2d_ctrl_reset_filter(&pos_ctrl->_pid_vel_xy);
    pid_2d_ctrl_reset_I(&pos_ctrl->_pid_vel_xy);
}

/// update_xy_controller - runs the horizontal position controller correcting position, velocity and acceleration errors.
///     Position and velocity errors are converted to velocity and acceleration targets using PID objects
///     Desired velocity and accelerations are added to these corrections as they are calculated
///     Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
void posctrl_update_xy_controller(Position_ctrl* pos_ctrl)
{
    //TODO: check for ekf xy position reset
    //handle_ekf_xy_reset();

    // Check for position control time out
    if (!posctrl_is_active_xy(pos_ctrl)) {
        posctrl_init_xy_controller(pos_ctrl);
        if (posctrl_has_good_timing()) {
            // call internal error because initialisation has not been done
            //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        }
    }
    pos_ctrl->_last_update_xy_us = time_micros64();

    //float ahrsGndSpdLimit, ahrsControlScaleXY;
    //AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);

    float ahrsGndSpdLimit = pos_ctrl->_psc_data.ahrsGndSpdLimit;
    float ahrsControlScaleXY = pos_ctrl->_psc_data.ahrsControlScaleXY;

    // Position Controller

    const Vector3f_t *curr_pos = &pos_ctrl->_ahrs->relpos_cm;
    Vector2f_t vel_target = p_2d_ctrl_update_all2(&pos_ctrl->_p_pos_xy, &pos_ctrl->_pos_target.x, 
                            &pos_ctrl->_pos_target.y, curr_pos, &pos_ctrl->_limit.pos_xy);
    
    // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
    vec2_mult(&vel_target, &vel_target, ahrsControlScaleXY);
    
    pos_ctrl->_vel_target.x = vel_target.x;
    pos_ctrl->_vel_target.y = vel_target.y;
    
    pos_ctrl->_vel_target.x += pos_ctrl->_vel_desired.x;
    pos_ctrl->_vel_target.y += pos_ctrl->_vel_desired.y;

    // Velocity Controller

    // check if vehicle velocity is being overridden
    // todo: remove this and use input shaping
    if (pos_ctrl->_flags.vehicle_horiz_vel_override) {
        pos_ctrl->_flags.vehicle_horiz_vel_override = false;
    } else {
        pos_ctrl->_vehicle_horiz_vel.x = pos_ctrl->_ahrs->velocity_cm.x;
        pos_ctrl->_vehicle_horiz_vel.y = pos_ctrl->_ahrs->velocity_cm.y;
    }

    Vector2f_t accel_target = pid_2d_update_all3(&pos_ctrl->_pid_vel_xy, &pos_ctrl->_vel_target,
                                               &pos_ctrl->_vehicle_horiz_vel, &pos_ctrl->_limit_vector);
    // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
    vec2_mult(&accel_target, &accel_target, ahrsControlScaleXY);

    // pass the correction acceleration to the target acceleration output
    pos_ctrl->_accel_target.x = accel_target.x;
    pos_ctrl->_accel_target.y = accel_target.y;

    // Add feed forward into the target acceleration output
    pos_ctrl->_accel_target.x += pos_ctrl->_accel_desired.x;
    pos_ctrl->_accel_target.y += pos_ctrl->_accel_desired.y;

    // Acceleration Controller

    // limit acceleration using maximum lean angles
    vec2_zero((Vector2f_t*)&pos_ctrl->_limit_vector);
    float angle_max = MIN(attctrl_get_althold_lean_angle_max_cd(pos_ctrl->_attitude_control), posctrl_get_lean_angle_max_cd(pos_ctrl));
    float accel_max = GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f));
    if (vec3_limit_length_xy(&pos_ctrl->_accel_target, accel_max)) {
        pos_ctrl->_limit_vector.x = pos_ctrl->_accel_target.x;
        pos_ctrl->_limit_vector.y = pos_ctrl->_accel_target.y;
    }

    // update angle targets that will be passed to stabilize controller
    posctrl_accel_to_lean_angles(pos_ctrl, pos_ctrl->_accel_target.x, pos_ctrl->_accel_target.y,
                                 &pos_ctrl->_roll_target, &pos_ctrl->_pitch_target);
    posctrl_calculate_yaw_and_rate_yaw(pos_ctrl);

    posctrl_publish_msg_xy(pos_ctrl);
}

// get_lean_angles_to_accel - convert roll, pitch lean angles to NE frame accelerations in cm/s/s
void posctrl_accel_to_lean_angles(Position_ctrl* pos_ctrl, float accel_x_cmss, float accel_y_cmss, float* roll_target, float* pitch_target)
{
    // rotate accelerations into body forward-right frame
    const float accel_forward = accel_x_cmss * pos_ctrl->_ahrs->cos_yaw + accel_y_cmss * pos_ctrl->_ahrs->sin_yaw;
    const float accel_right = -accel_x_cmss * pos_ctrl->_ahrs->sin_yaw + accel_y_cmss * pos_ctrl->_ahrs->cos_yaw;

    // update angle targets that will be passed to stabilize controller
    *pitch_target = atanf(-accel_forward / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
    float cos_pitch_target = cosf(*pitch_target * M_PI / 18000.0f);
    *roll_target = atanf(accel_right * cos_pitch_target / (GRAVITY_MSS * 100.0f)) * (18000.0f / M_PI);
}

// calculate_yaw_and_rate_yaw - update the calculated the vehicle yaw and rate of yaw.
bool posctrl_calculate_yaw_and_rate_yaw(Position_ctrl* pos_ctrl)
{
    // Calculate the turn rate
    float turn_rate = 0.0f;
    const float vel_desired_xy_len = vec2_length((Vector2f_t*)&pos_ctrl->_vel_desired);
    if (math_flt_positive(vel_desired_xy_len)) {
        const float accel_forward = (pos_ctrl->_accel_desired.x * pos_ctrl->_vel_desired.x + pos_ctrl->_accel_desired.y * pos_ctrl->_vel_desired.y) / vel_desired_xy_len;
        const Vector2f_t accel_turn = {pos_ctrl->_accel_desired.x - pos_ctrl->_vel_desired.x * accel_forward / vel_desired_xy_len,
                                       pos_ctrl->_accel_desired.y - pos_ctrl->_vel_desired.y * accel_forward / vel_desired_xy_len,};
        const float accel_turn_xy_len = vec2_length(&accel_turn);
        turn_rate = accel_turn_xy_len / vel_desired_xy_len;
        if ((accel_turn.y * pos_ctrl->_vel_desired.x - accel_turn.x * pos_ctrl->_vel_desired.y) < 0.0) {
            turn_rate = -turn_rate;
        }
    }

    // update the target yaw if velocity is greater than 5% _vel_max_xy_cms
    if (vel_desired_xy_len > pos_ctrl->_vel_max_xy_cms * 0.05f) {
        pos_ctrl->_yaw_target = degrees(vec3_angle_xy(&pos_ctrl->_vel_desired)) * 100.0f;
        pos_ctrl->_yaw_rate_target = turn_rate * degrees(100.0f);
        return true;
    }
    return false;
}

/// get_lean_angle_max_cd - returns the maximum lean angle the autopilot may request
float posctrl_get_lean_angle_max_cd(Position_ctrl* pos_ctrl)
{
    if (math_flt_zero(pos_ctrl->_lean_angle_max)) {
        return attctrl_lean_angle_max_cd(pos_ctrl->_attitude_control);
    }
    return pos_ctrl->_lean_angle_max * 100.0f;
}

// return true if on a real vehicle or SITL with lock-step scheduling
bool posctrl_has_good_timing()
{
    // real boards are assumed to have good timing
    return true;
}

// lean_angles_to_accel_xy - convert roll, pitch lean target angles to NE frame accelerations in cm/s/s
// todo: this should be based on thrust vector attitude control
void posctrl_lean_angles_to_accel_xy(Position_ctrl* pos_ctrl, float* accel_x_cmss, float* accel_y_cmss)
{
    // rotate our roll, pitch angles into lat/lon frame
    Vector3f_t att_target_euler = attctrl_get_att_target_euler_rad(pos_ctrl->_attitude_control);
    att_target_euler.z = pos_ctrl->_ahrs->yaw;
    Vector3f_t accel_cmss = posctrl_lean_angles_to_accel(&att_target_euler);

    *accel_x_cmss = accel_cmss.x;
    *accel_y_cmss = accel_cmss.y;
}

// get_lean_angles_to_accel - convert roll, pitch lean target angles to lat/lon frame accelerations in cm/s/s
Vector3f_t posctrl_lean_angles_to_accel(const Vector3f_t* att_target_euler)
{
    // rotate our roll, pitch angles into lat/lon frame
    const float sin_roll = sinf(att_target_euler->x);
    const float cos_roll = cosf(att_target_euler->x);
    const float sin_pitch = sinf(att_target_euler->y);
    const float cos_pitch = cosf(att_target_euler->y);
    const float sin_yaw = sinf(att_target_euler->z);
    const float cos_yaw = cosf(att_target_euler->z);

    Vector3f_t lean_angles_to_accel = {
        (GRAVITY_MSS * 100) * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
        (GRAVITY_MSS * 100) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
        (GRAVITY_MSS * 100)
    };

    return lean_angles_to_accel;
}

/// initialise ekf xy position reset check
void posctrl_init_ekf_xy_reset(Position_ctrl* pos_ctrl)
{
    static bool first_run = true;
    //pos_ctrl->_ekf_xy_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);

    // 临时措施，20210830
    if (first_run) {
        pos_ctrl->_ekf_xy_reset_ms = time_millis();
        first_run = false;
    }
}

/// set_max_speed_accel_z - set the maximum vertical speed in cm/s and acceleration in cm/s/s
///     speed_down can be positive or negative but will always be interpreted as a descent speed.
///     This function only needs to be called if using the kinematic shaping.
///     This can be done at any time as changes in these parameters are handled smoothly
///     by the kinematic shaping.
void posctrl_set_max_speed_accel_z(Position_ctrl* pos_ctrl, float speed_down, float speed_up, float accel_cmss)
{
    // ensure speed_down is always negative
    speed_down = -fabsf(speed_down);

    // sanity check and update
    if (math_flt_negative(speed_down)) {
        pos_ctrl->_vel_max_down_cms = speed_down;
    }
    if (math_flt_positive(speed_up)) {
        pos_ctrl->_vel_max_up_cms = speed_up;
    }
    if (math_flt_positive(accel_cmss)) {
        pos_ctrl->_accel_max_z_cmss = accel_cmss;
    }

    // ensure the vertical Jerk is not limited by the filters in the Z accel PID object
    pos_ctrl->_jerk_max_z_cmsss = pos_ctrl->_shaping_jerk_z * 100.0;
    if (math_flt_positive(pos_ctrl->_pid_accel_z._filt_t_hz)) {
        pos_ctrl->_jerk_max_z_cmsss = MIN(pos_ctrl->_jerk_max_z_cmsss, MIN(GRAVITY_MSS * 100.0f, pos_ctrl->_accel_max_z_cmss) * (M_2PI * pos_ctrl->_pid_accel_z._filt_t_hz) / 5.0f);
    }
    if (math_flt_positive(pos_ctrl->_pid_accel_z._filt_e_hz)) {
        pos_ctrl->_jerk_max_z_cmsss = MIN(pos_ctrl->_jerk_max_z_cmsss, MIN(GRAVITY_MSS * 100.0f, pos_ctrl->_accel_max_z_cmss) * (M_2PI * pos_ctrl->_pid_accel_z._filt_e_hz) / 5.0f);
    }
}

/// set_correction_speed_accel_z - set the position controller correction velocity and acceleration limit
///     speed_down can be positive or negative but will always be interpreted as a descent speed.
///     This should be done only during initialisation to avoid discontinuities
void posctrl_set_correction_speed_accel_z(Position_ctrl* pos_ctrl, float speed_down, float speed_up, float accel_cmss)
{
    // define maximum position error and maximum first and second differential limits
    p_1d_ctrl_set_limits(&pos_ctrl->_p_pos_z, -fabsf(speed_down), pos_ctrl->_vel_max_up_cms, pos_ctrl->_accel_max_z_cmss, 0.0f);
}

/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
void posctrl_init_z_controller(Position_ctrl* pos_ctrl)
{
    // Initialise the position controller to the current position, velocity and acceleration.
    posctrl_init_z(pos_ctrl);

    // Set accel PID I term based on the current throttle
    pid_ctrl_set_integrator1(&pos_ctrl->_pid_accel_z, 
                            (attctrl_get_throttle_in(pos_ctrl->_attitude_control)
                            - MotorsMC_get_throttle_hover((MotorsMC_HandleTypeDef *)pos_ctrl->_motors) * 1000.0f));
}

/// init_z_controller - initialise the position controller to the current position, velocity, acceleration and attitude.
///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
///     This function does not allow any negative velocity or acceleration
void posctrl_init_z_controller_no_descent(Position_ctrl* pos_ctrl)
{
    // Initialise the position controller to the current throttle, position, velocity and acceleration.
    posctrl_init_z_controller(pos_ctrl);

    // remove all descent if present
    pos_ctrl->_vel_desired.z = MAX(0.0, pos_ctrl->_vel_desired.z);
    pos_ctrl->_vel_target.z = MAX(0.0, pos_ctrl->_vel_target.z);
    pos_ctrl->_accel_desired.z = MAX(0.0, pos_ctrl->_accel_desired.z);
    pos_ctrl->_accel_target.z = MAX(0.0, pos_ctrl->_accel_target.z);
}

/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
///     This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position.
///     The starting position can be retrieved by getting the position target using get_pos_target_cm() after calling this function.
void posctrl_init_z_controller_stopping_point(Position_ctrl* pos_ctrl)
{
    // Initialise the position controller to the current throttle, position, velocity and acceleration.
    posctrl_init_z_controller(pos_ctrl);

    posctrl_get_stopping_point_z_cm(pos_ctrl, &pos_ctrl->_pos_target.z);
    pos_ctrl->_vel_desired.z = 0.0f;
    pos_ctrl->_accel_desired.z = 0.0f;
}

// relax_z_controller - initialise the position controller to the current position and velocity with decaying acceleration.
///     This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration.
void posctrl_relax_z_controller(Position_ctrl* pos_ctrl, float throttle_setting)
{
    // Initialise the position controller to the current position, velocity and acceleration.
    posctrl_init_z(pos_ctrl);

    // Set accel PID I term based on the requested throttle
    float throttle = attctrl_get_throttle_in(pos_ctrl->_attitude_control);
    throttle_setting = throttle + (throttle_setting - throttle) * (pos_ctrl->_dt / (pos_ctrl->_dt + POSCONTROL_RELAX_TC));

    // Set accel PID I term based on the current throttle
    pid_ctrl_set_integrator1(&pos_ctrl->_pid_accel_z, 
                            (throttle_setting - MotorsMC_get_throttle_hover((MotorsMC_HandleTypeDef *)pos_ctrl->_motors) * 1000.0f));
}

/// init_z - initialise the position controller to the current position, velocity and acceleration.
///     This function is private and contains all the shared z axis initialisation functions
void posctrl_init_z(Position_ctrl* pos_ctrl)
{
    pos_ctrl->_pos_target.z = pos_ctrl->_ahrs->relpos_cm.z;

    pos_ctrl->_vel_desired.z = pos_ctrl->_ahrs->velocity_cm.z;
    // with zero position error _vel_target = _vel_desired
    pos_ctrl->_vel_target.z = pos_ctrl->_ahrs->velocity_cm.z;

    const Vector3f_t *curr_accel = &pos_ctrl->_ahrs->accel_ef;

    // Reset I term of velocity PID
    pid_basic_ctrl_reset_filter(&pos_ctrl->_pid_vel_z);
    pid_basic_ctrl_set_integrator(&pos_ctrl->_pid_vel_z, 0.0f);

    pos_ctrl->_accel_desired.z = math_constrain_float(-(curr_accel->z + GRAVITY_MSS) * 100.0f, -pos_ctrl->_accel_max_z_cmss, pos_ctrl->_accel_max_z_cmss);
    pos_ctrl->_accel_target.z = pos_ctrl->_accel_desired.z;
    pid_ctrl_reset_filter(&pos_ctrl->_pid_accel_z);

    // initialise vertical offsets
    pos_ctrl->_pos_offset_target_z = 0.0f;
    pos_ctrl->_pos_offset_z = 0.0f;
    pos_ctrl->_vel_offset_z = 0.0f;
    pos_ctrl->_accel_offset_z = 0.0f;

    // initialise ekf z reset handler
    posctrl_init_ekf_xy_reset(pos_ctrl);

    // initialise z_controller time out
    pos_ctrl->_last_update_z_us = time_micros64();
}

/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
///     The vel is projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The function alters the vel to be the kinematic path based on accel
void posctrl_input_accel_z(Position_ctrl* pos_ctrl, const float accel)
{
    // calculated increased maximum acceleration if over speed
    float accel_z_cmss = pos_ctrl->_accel_max_z_cmss;
    if (pos_ctrl->_vel_desired.z < pos_ctrl->_vel_max_down_cms && !math_flt_zero(pos_ctrl->_vel_max_down_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_down_cms;
    }
    if (pos_ctrl->_vel_desired.z > pos_ctrl->_vel_max_up_cms && !math_flt_zero(pos_ctrl->_vel_max_up_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_up_cms;
    }

    // adjust desired alt if motors have not hit their limits
    control_update_pos_vel_accel(&pos_ctrl->_pos_target.z, 
                                 &pos_ctrl->_vel_desired.z, 
                                 pos_ctrl->_accel_desired.z, 
                                 pos_ctrl->_dt, 
                                 pos_ctrl->_limit_vector.z);

    control_shape_accel(accel, &pos_ctrl->_accel_desired.z, pos_ctrl->_jerk_max_z_cmsss, pos_ctrl->_dt);
}

/// input_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z.
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_vel_accel_z(Position_ctrl* pos_ctrl, float *vel, const float accel, bool ignore_descent_limit, bool limit_output)
{
    if (ignore_descent_limit) {
        // turn off limits in the negative z direction
        pos_ctrl->_limit_vector.z = MAX(pos_ctrl->_limit_vector.z, 0.0f);
    }

    // calculated increased maximum acceleration if over speed
    float accel_z_cmss = pos_ctrl->_accel_max_z_cmss;
    if (pos_ctrl->_vel_desired.z < pos_ctrl->_vel_max_down_cms && !math_flt_zero(pos_ctrl->_vel_max_down_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_down_cms;
    }
    if (pos_ctrl->_vel_desired.z > pos_ctrl->_vel_max_up_cms && !math_flt_zero(pos_ctrl->_vel_max_up_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_up_cms;
    }

    // adjust desired alt if motors have not hit their limits
    control_update_pos_vel_accel(&pos_ctrl->_pos_target.z, 
                                 &pos_ctrl->_vel_desired.z, 
                                 pos_ctrl->_accel_desired.z, 
                                 pos_ctrl->_dt, 
                                 pos_ctrl->_limit_vector.z);

    control_shape_vel_accel(*vel, accel,
                            pos_ctrl->_vel_desired.z, &pos_ctrl->_accel_desired.z,
                            -math_constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
                            pos_ctrl->_jerk_max_z_cmsss, pos_ctrl->_dt, limit_output);

    control_update_vel_accel(vel, accel, pos_ctrl->_dt, 0);
}

/// set_pos_target_z_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
///     using the default position control kinematic path.
///     The zero target altitude is varied to follow pos_offset_z
void posctrl_set_pos_target_z_from_climb_rate_cm(Position_ctrl* pos_ctrl, float vel)
{
    // remove terrain offsets for flat earth assumption
    pos_ctrl->_pos_target.z -= pos_ctrl->_pos_offset_z;
    pos_ctrl->_vel_desired.z -= pos_ctrl->_vel_offset_z;
    pos_ctrl->_accel_desired.z -= pos_ctrl->_accel_offset_z;

    float vel_temp = vel;
    posctrl_input_vel_accel_z(pos_ctrl, &vel_temp, 0, false, true);

    // update the vertical position, velocity and acceleration offsets
    posctrl_update_pos_offset_z(pos_ctrl, pos_ctrl->_pos_offset_target_z);

    // add terrain offsets
    pos_ctrl->_pos_target.z += pos_ctrl->_pos_offset_z;
    pos_ctrl->_vel_desired.z += pos_ctrl->_vel_offset_z;
    pos_ctrl->_accel_desired.z += pos_ctrl->_accel_offset_z;
}

/// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
///     using the default position control kinematic path.
///     ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be false unless landing.
void posctrl_land_at_climb_rate_cm(Position_ctrl* pos_ctrl, float vel, bool ignore_descent_limit)
{
    posctrl_input_vel_accel_z(pos_ctrl, &vel, 0, ignore_descent_limit, true);
}

/// input_pos_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration.
///     The pos and vel are projected forwards in time based on a time step of dt and acceleration accel.
///     The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt.
///     The function alters the pos and vel to be the kinematic path based on accel
///     The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction.
void posctrl_input_pos_vel_accel_z(Position_ctrl* pos_ctrl, float *pos, float *vel, const float accel, bool limit_output)
{
    // calculated increased maximum acceleration if over speed
    float accel_z_cmss = pos_ctrl->_accel_max_z_cmss;
    if (pos_ctrl->_vel_desired.z < pos_ctrl->_vel_max_down_cms && !math_flt_zero(pos_ctrl->_vel_max_down_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_down_cms;
    }
    if (pos_ctrl->_vel_desired.z > pos_ctrl->_vel_max_up_cms && !math_flt_zero(pos_ctrl->_vel_max_up_cms)) {
        accel_z_cmss *= POSCONTROL_OVERSPEED_GAIN_Z * pos_ctrl->_vel_desired.z / pos_ctrl->_vel_max_up_cms;
    }

    // adjust desired alt if motors have not hit their limits
    control_update_pos_vel_accel(&pos_ctrl->_pos_target.z, 
                                 &pos_ctrl->_vel_desired.z, 
                                 pos_ctrl->_accel_desired.z, 
                                 pos_ctrl->_dt, 
                                 pos_ctrl->_limit_vector.z);
    
    control_shape_pos_vel_accel(*pos, *vel, accel,
                        pos_ctrl->_pos_target.z, pos_ctrl->_vel_desired.z, &pos_ctrl->_accel_desired.z,
                        pos_ctrl->_vel_max_down_cms, pos_ctrl->_vel_max_up_cms,
                        -math_constrain_float(accel_z_cmss, 0.0f, 750.0f), accel_z_cmss,
                        pos_ctrl->_jerk_max_z_cmsss, pos_ctrl->_dt, limit_output);

    postype_t posp = *pos;
    control_update_pos_vel_accel(&posp, vel, accel, pos_ctrl->_dt, 0);
    *pos = posp;
}

/// set_alt_target_with_slew - adjusts target up or down using a commanded altitude in cm
///     using the default position control kinematic path.
void posctrl_set_alt_target_with_slew(Position_ctrl* pos_ctrl, const float pos)
{
    float posf = pos;
    float zero = 0.0f;
    posctrl_input_pos_vel_accel_z(pos_ctrl, &posf, &zero, 0, true);
}

/// update_pos_offset_z - updates the vertical offsets used by terrain following
void posctrl_update_pos_offset_z(Position_ctrl* pos_ctrl, float pos_offset_z)
{

    postype_t p_offset_z = pos_ctrl->_pos_offset_z;
    control_update_pos_vel_accel(&p_offset_z, &pos_ctrl->_vel_offset_z, pos_ctrl->_accel_offset_z, pos_ctrl->_dt, MIN(pos_ctrl->_limit_vector.z, 0.0f));
    pos_ctrl->_pos_offset_z = p_offset_z;

    // input shape the terrain offset
    control_shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f,
        pos_ctrl->_pos_offset_z, pos_ctrl->_vel_offset_z, &pos_ctrl->_accel_offset_z,
        posctrl_get_max_speed_down_cms(pos_ctrl), posctrl_get_max_speed_up_cms(pos_ctrl),
        -posctrl_get_max_accel_z_cmss(pos_ctrl), posctrl_get_max_accel_z_cmss(pos_ctrl),
        pos_ctrl->_jerk_max_z_cmsss, pos_ctrl->_dt, false);
}

/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
float posctrl_pos_offset_z_scaler(Position_ctrl* pos_ctrl, float pos_offset_z, float pos_offset_z_buffer)
{
    if (math_flt_zero(pos_offset_z_buffer)) {
        return 1.0;
    }
    const Vector3f_t curr_pos = pos_ctrl->_ahrs->relpos_cm;
    float pos_offset_error_z = curr_pos.z - (pos_ctrl->_pos_target.z - pos_ctrl->_pos_offset_z + pos_offset_z);
    return math_constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0);
}

/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void posctrl_get_stopping_point_z_cm(Position_ctrl* pos_ctrl, postype_t *stopping_point)
{
    const float curr_pos_z = pos_ctrl->_ahrs->relpos_cm.z;
    float curr_vel_z = pos_ctrl->_ahrs->velocity_cm.z;

    // avoid divide by zero by using current position if kP is very low or acceleration is zero
    if (!math_flt_positive(pos_ctrl->_p_pos_z._kp) || !math_flt_positive(pos_ctrl->_accel_max_z_cmss)) {
        *stopping_point = curr_pos_z;
        return;
    }

    *stopping_point = curr_pos_z + math_constrain_float(control_stopping_distance(curr_vel_z, pos_ctrl->_p_pos_z._kp, pos_ctrl->_accel_max_z_cmss), - POSCONTROL_STOPPING_DIST_DOWN_MAX, POSCONTROL_STOPPING_DIST_UP_MAX);
}

// is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool posctrl_is_active_z(Position_ctrl* pos_ctrl)
{
    return ((time_micros64() - pos_ctrl->_last_update_z_us) <= pos_ctrl->_dt * 5000000.0f);
}

/// update_z_controller - runs the vertical position controller correcting position, velocity and acceleration errors.
///     Position and velocity errors are converted to velocity and acceleration targets using PID objects
///     Desired velocity and accelerations are added to these corrections as they are calculated
///     Kinematically consistent target position and desired velocity and accelerations should be provided before calling this function
void posctrl_update_z_controller(Position_ctrl* pos_ctrl)
{
    // TODO: check for ekf z-axis position reset
    //handle_ekf_z_reset();

    // Check for z_controller time out
    if (!posctrl_is_active_z(pos_ctrl)) {
        posctrl_init_z_controller(pos_ctrl);
        if (posctrl_has_good_timing()) {
            // call internal error because initialisation has not been done
            //INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
        }
    }
    pos_ctrl->_last_update_z_us = time_micros64();

    const float curr_alt = pos_ctrl->_ahrs->relpos_cm.z;
    // calculate the target velocity correction
    float pos_target_zf = pos_ctrl->_pos_target.z;

    pos_ctrl->_vel_target.z = p_1d_ctrl_update_all(&pos_ctrl->_p_pos_z,
                                                   &pos_target_zf, curr_alt,
                                                   &pos_ctrl->_limit.pos_down,
                                                   &pos_ctrl->_limit.pos_up);
    
    pos_ctrl->_vel_target.z *= pos_ctrl->_psc_data.controlScaleZ;

    pos_ctrl->_pos_target.z = pos_target_zf;

    // add feed forward component
    pos_ctrl->_vel_target.z += pos_ctrl->_vel_desired.z;

    // Velocity Controller

    const Vector3f_t* curr_vel = &pos_ctrl->_ahrs->velocity_cm;
    pos_ctrl->_accel_target.z = pid_basic_ctrl_update_all2(&pos_ctrl->_pid_vel_z,
                                                          pos_ctrl->_vel_target.z, curr_vel->z,
                                                          pos_ctrl->_motors->limit.throttle_lower,
                                                          pos_ctrl->_motors->limit.throttle_upper);
    
    pos_ctrl->_accel_target.z *= pos_ctrl->_psc_data.controlScaleZ;

    // add feed forward component
    pos_ctrl->_accel_target.z += pos_ctrl->_accel_desired.z;

    // Acceleration Controller

    // Calculate vertical acceleration
    const float z_accel_meas = -(pos_ctrl->_ahrs->accel_ef.z + GRAVITY_MSS) * 100.0f;

    // ensure imax is always large enough to overpower hover throttle
    if (MotorsMC_get_throttle_hover((MotorsMC_HandleTypeDef *)pos_ctrl->_motors) * 1000.0f > pid_ctrl_get_kimax(&pos_ctrl->_pid_accel_z)) {
        pid_ctrl_set_kimax(&pos_ctrl->_pid_accel_z, MotorsMC_get_throttle_hover((MotorsMC_HandleTypeDef *)pos_ctrl->_motors) * 1000.0f);
    }
    float thr_out;
    if (pos_ctrl->_vibe_comp_enabled) {
        thr_out = posctrl_get_throttle_with_vibration_override(pos_ctrl);
    } else {
        thr_out = pid_ctrl_update_all(&pos_ctrl->_pid_accel_z, pos_ctrl->_accel_target.z, z_accel_meas, (pos_ctrl->_motors->limit.throttle_lower || pos_ctrl->_motors->limit.throttle_upper)) * 0.001f;
        thr_out += pos_ctrl->_pid_accel_z._kff * 0.001f;
    }
    thr_out += MotorsMC_get_throttle_hover((MotorsMC_HandleTypeDef *)pos_ctrl->_motors);

    // Actuator commands

    // send throttle to attitude controller with angle boost
    attctrl_set_throttle_out(pos_ctrl->_attitude_control, thr_out, true, POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ);

    // Check for vertical controller health

    // _speed_down_cms is checked to be non-zero when set
    float error_ratio = pos_ctrl->_pid_vel_z._error / pos_ctrl->_vel_max_down_cms;
    pos_ctrl->_vel_z_control_ratio += pos_ctrl->_dt * 0.1f * (0.5 - error_ratio);
    pos_ctrl->_vel_z_control_ratio = math_constrain_float(pos_ctrl->_vel_z_control_ratio, 0.0f, 2.0f);

    // set vertical component of the limit vector
    if (pos_ctrl->_motors->limit.throttle_upper) {
        pos_ctrl->_limit_vector.z = 1.0f;
    } else if (pos_ctrl->_motors->limit.throttle_lower) {
        pos_ctrl->_limit_vector.z = -1.0f;
    } else {
        pos_ctrl->_limit_vector.z = 0.0f;
    }

    posctrl_publish_msg_z(pos_ctrl);
}

// get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain)
float posctrl_get_throttle_with_vibration_override(Position_ctrl* pos_ctrl)
{
    const float thr_per_accelz_cmss = MotorsMC_get_throttle_hover((MotorsMC_HandleTypeDef *)pos_ctrl->_motors) / (GRAVITY_MSS * 100.0f);
    // during vibration compensation use feed forward with manually calculated gain
    // ToDo: clear pid_info P, I and D terms for logging
    if (!(pos_ctrl->_motors->limit.throttle_lower || pos_ctrl->_motors->limit.throttle_upper)
        || ((math_flt_positive(pos_ctrl->_pid_accel_z.integrator) && math_flt_negative(pos_ctrl->_pid_vel_z._error))
        || (math_flt_negative(pos_ctrl->_pid_accel_z.integrator) && math_flt_positive(pos_ctrl->_pid_vel_z._error)))) {
        pid_ctrl_set_integrator1(&pos_ctrl->_pid_accel_z, pos_ctrl->_pid_accel_z.integrator + pos_ctrl->_dt * thr_per_accelz_cmss * 1000.0f * pos_ctrl->_pid_vel_z._error * pos_ctrl->_pid_vel_z._kp * POSCONTROL_VIBE_COMP_I_GAIN);
    }

    return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accelz_cmss * pos_ctrl->_accel_target.z + pos_ctrl->_pid_accel_z.integrator * 0.001f;
}


///
/// Accessors
///

/// set position, velocity and acceleration targets
void posctrl_set_pos_vel_accel(Position_ctrl* pos_ctrl, const Vector3p* pos, const Vector3f_t* vel, const Vector3f_t* accel)
{
    pos_ctrl->_pos_target = *pos;
    pos_ctrl->_vel_desired = *vel;
    pos_ctrl->_accel_desired = *accel;
}

/// set position, velocity and acceleration targets
void posctrl_set_pos_vel_accel_xy(Position_ctrl* pos_ctrl, const Vector2p* pos, const Vector2f_t* vel, const Vector2f_t* accel)
{
    pos_ctrl->_pos_target.x = pos->x;
    pos_ctrl->_pos_target.y = pos->y;
    pos_ctrl->_vel_desired.x = vel->x;
    pos_ctrl->_vel_desired.y = vel->y;
    pos_ctrl->_accel_desired.x = accel->x;
    pos_ctrl->_accel_desired.y = accel->y;
}

/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
///    function does not change the z axis
void posctrl_get_stopping_point_xy_cm(Position_ctrl* pos_ctrl, Vector2p *stopping_point)
{
    const Vector3f_t* curr_pos = &pos_ctrl->_ahrs->relpos_cm;
    stopping_point->x = curr_pos->x;
    stopping_point->y = curr_pos->y;
    float kP = pos_ctrl->_p_pos_xy._kp;

    Vector2f_t* curr_vel = (Vector2f_t*)&pos_ctrl->_ahrs->velocity_cm;

    // calculate current velocity
    float vel_total = vec2_length(curr_vel);

    if (!math_flt_positive(vel_total)) {
        return;
    }

    const float stopping_dist = control_stopping_distance(math_constrain_float(vel_total, 0.0, pos_ctrl->_vel_max_xy_cms), kP, pos_ctrl->_accel_max_xy_cmss);
    if (!math_flt_positive(stopping_dist)) {
        return;
    }

    // convert the stopping distance into a stopping point using velocity vector
    const float t = stopping_dist / vel_total;
    
    stopping_point->x += (curr_vel->x * t);
    stopping_point->y += (curr_vel->y * t);
}


// returns the NED target acceleration vector for attitude control
Vector3f_t posctrl_get_thrust_vector(Position_ctrl* pos_ctrl)
{
    Vector3f_t accel_target = posctrl_get_accel_target_cmss(pos_ctrl);
    accel_target.z = -GRAVITY_MSS * 100.0f;
    return accel_target;
}

/// get_bearing_to_target_cd - get bearing to target position in centi-degrees
int32_t posctrl_get_bearing_to_target_cd(Position_ctrl* pos_ctrl)
{
    Vector3f_t pos_target = {pos_ctrl->_pos_target.x, pos_ctrl->_pos_target.y, pos_ctrl->_pos_target.z};
    return vec3_get_bearing_cd(&pos_ctrl->_ahrs->relpos_cm, &pos_target);
}

/// crosstrack_error - returns horizontal error to the closest point to the current track
float posctrl_crosstrack_error(Position_ctrl* pos_ctrl)
{
    const Vector3f_t* curr_pos = &pos_ctrl->_ahrs->relpos_cm;
    const Vector2f_t pos_error = {curr_pos->x - pos_ctrl->_pos_target.x,
                                  curr_pos->y - pos_ctrl->_pos_target.y};
    if (math_flt_zero(vec2_length_squared((Vector2f_t*)&pos_ctrl->_vel_desired))) {
        // crosstrack is the horizontal distance to target when stationary
        return vec2_length(&pos_error);
    } else {
        // crosstrack is the horizontal distance to the closest point to the current track
        const Vector2f_t vel_unit = vec2_normalized((Vector2f_t*)&pos_ctrl->_vel_desired);
        const float dot_error = vec2_dot(&pos_error, &vel_unit);

        // todo: remove MAX of zero when safe_sqrt fixed
        return math_sqrtf(MAX(vec2_length_squared(&pos_error) - sq(dot_error), 0.0f));
    }
}

/// standby_xyz_reset - resets I terms and removes position error
///     This function will let Loiter and Alt Hold continue to operate
///     in the event that the flight controller is in control of the
///     aircraft when in standby.
void posctrl_standby_xyz_reset(Position_ctrl* pos_ctrl)
{
    // Set _pid_accel_z integrator to zero.
    pid_ctrl_set_integrator1(&pos_ctrl->_pid_accel_z, 0.0f);

    // Set the target position to the current pos.
    pos_ctrl->_pos_target.x = pos_ctrl->_ahrs->relpos_cm.x;
    pos_ctrl->_pos_target.y = pos_ctrl->_ahrs->relpos_cm.y;
    pos_ctrl->_pos_target.z = pos_ctrl->_ahrs->relpos_cm.z;

    // Set _pid_vel_xy integrator and derivative to zero.
    pid_2d_ctrl_reset_filter(&pos_ctrl->_pid_vel_xy);

    // initialise ekf xy reset handler
    posctrl_init_ekf_xy_reset(pos_ctrl);
}


/**
  * @brief       
  * @param[in]   pos_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
void posctrl_publish_msg_xy(Position_ctrl* pos_ctrl)
{
    uitc_vehicle_pos_ctrl_xy vehicle_pos_ctrl_xy = {0};

    vehicle_pos_ctrl_xy.timestamp_us = time_micros64();

    vehicle_pos_ctrl_xy.target_pos_xy[0] = pos_ctrl->_pos_target.x;    //单位：cm
    vehicle_pos_ctrl_xy.target_pos_xy[1] = pos_ctrl->_pos_target.y;    //单位：cm
    
    vehicle_pos_ctrl_xy.target_vel_xy[0] = pos_ctrl->_vel_target.x;    //单位：cm/s
    vehicle_pos_ctrl_xy.target_vel_xy[1] = pos_ctrl->_vel_target.y;    //单位：cm/s
    
    vehicle_pos_ctrl_xy.target_acc_xy[0] = pos_ctrl->_accel_target.x;    //单位：cm/s/s
    vehicle_pos_ctrl_xy.target_acc_xy[1] = pos_ctrl->_accel_target.y;    //单位：cm/s/s

    itc_publish(ITC_ID(vehicle_pos_ctrl_xy), &vehicle_pos_ctrl_xy);
}

/**
  * @brief       
  * @param[in]   pos_ctrl  
  * @param[out]  
  * @retval      
  * @note        
  */
void posctrl_publish_msg_z(Position_ctrl* pos_ctrl)
{
    uitc_vehicle_pos_ctrl_z pos_ctrl_z = {0};

    pos_ctrl_z.timestamp_us = time_micros64();

    pos_ctrl_z.target_pos_z = pos_ctrl->_pos_target.z;    //单位：cm
    pos_ctrl_z.target_vel_z = pos_ctrl->_vel_target.z;    //单位：cm/s
    pos_ctrl_z.target_acc_z = pos_ctrl->_accel_target.z;    //单位：cm/s/s

    itc_publish(ITC_ID(vehicle_pos_ctrl_z), &pos_ctrl_z);
}

/*------------------------------------test------------------------------------*/


